#ifndef PIDCTR_C
#define PIDCTR_C
#include "pidctr.h"

#define PD_DEFAULT 10
#define PI_DEFAULT 0.5
#define PP_DEFAULT 5

static float Kp,Ki,Kd; 
static float Pmax,Pmin,Emax;
static float E0,E_0,E_21,E_11;
static float deltaPn,deltaPi,deltaPp,deltaPd,PsumCopy,Psum1;


int Api_Pidctr_Get_Conf(PIDset *t)
{
	PIDset d;
	FILE *fp ;
	if((fp = fopen("/bin/pid.conf", "r")) == NULL){
		d.Pd=PD_DEFAULT;
		d.Pi=PI_DEFAULT;
		d.Pp=PP_DEFAULT;
	}
	else{
		fread(&d,sizeof(PIDset),1,fp);
		fclose(fp) ;
	}
	memcpy(t,&d,sizeof(PIDset));
	return -NO_ERROR;
}

int Api_Pidctr_Set_Conf(PIDset t)
{
	FILE *fp ;
	if((fp = fopen("/bin/pid.conf", "w"))==NULL)
		return -PIDCTR_CONF_WRITEFAILED;
	fwrite(&t,sizeof(PIDset),1,fp);
	fclose(fp);
	Api_Pidctr_Init();
	return -NO_ERROR;
}

int Api_Pidctr_Init()
{
	PIDset t;
	Pmax=100.0;
	Pmin=0.0;
	Emax=5.0;
	E0=1.0;
	E_0=0;
	E_21=0;
	E_11=0;
	deltaPn=0;
	deltaPi=0;
	deltaPp=0;
	deltaPd=0;
	PsumCopy=0;
	Psum1=0;
	Api_Pidctr_Get_Conf(&t);
	Kp=t.Pp;
	Ki=t.Pi;
	Kd=t.Pd;
	
}

float Api_Pidctr_Ctr(float ADCur,float ADSet,Ctr_Type type)
{
	if(type==CTR_COLD)
		E_0=ADCur-ADSet;
	else
		E_0=ADSet-ADCur;

	if(fabs(E_0)>Emax){
		deltaPp=Kp*(E_0-E_11);
		deltaPd=Kd*(E_0-2*E_11+E_21);
		deltaPn=deltaPp+deltaPd;
	}
	else{
		if(fabs(E_0)>E0)
			deltaPi=Ki*E_0*(Emax-E_0)/(Emax-E0);
		else
			deltaPi=Ki*E_0;
		deltaPp=Kp*(E_0-E_11);
		deltaPd=Kd*(E_0-2*E_11+E_21);
		deltaPn=deltaPp+deltaPi+deltaPd;
	}
	//  Psum1+=deltaPn;
	// PsumCopy=Psum1;
	PsumCopy+=deltaPn;
	if(PsumCopy>Pmax) PsumCopy=Pmax;
	if(PsumCopy<Pmin) PsumCopy=Pmin;
	/*  if(deltaPn>Pmax) deltaPn=Pmax;
	if(deltaPn<Pmin) deltaPn=Pmin;*/
	E_21=E_11;
	E_11=E_0;
	return PsumCopy;
}
#endif
